#include "hal_function.h"
#include "mpu6050.h"

static const char *TAG = "MPU6050";
MPU6050_DataHandle mpu6050Handle;

//===================================================================================================================
//====== Set of useful function to access acceleratio, gyroscope, and temperature data
//===================================================================================================================
static void delayms(uint16_t ms)
{
    vTaskDelay(pdMS_TO_TICKS(ms)); 
}

static void MPU6050_WriteByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
    i2c_write_mem(address,&subAddress,1,&data,1);
}


static void MPU6050_WriteBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{
    i2c_write_mem(address,&subAddress,1,dest,count);
}

static char MPU6050_ReadByte(uint8_t address, uint8_t subAddress)
{
    char data; // `data` will store the register data     
    i2c_read_mem(address,&subAddress,1,&data,1);
    return data; 
}

static void MPU6050_ReadBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{     
    i2c_read_mem(address,&subAddress,1,dest,count);
} 


static void MPU6050_ReadBits(uint8_t address, uint8_t subAddress,uint8_t start,uint8_t len,uint8_t *bits)
{   
    uint8_t datamask = 0xff;  
    uint8_t data = MPU6050_ReadByte(address,subAddress);
    datamask >>= (8 - len);
    *bits = ((data >> start)& datamask);
} 

static void MPU6050_WriteBits(uint8_t address, uint8_t subAddress,uint8_t start,uint8_t len,uint8_t bits)
{   
    uint8_t datamask = 0xff;
    uint8_t data;
    datamask >>= (8 - len);
    data = MPU6050_ReadByte(address,subAddress);
    data &= (~(datamask << start));
    data |= ((bits&datamask) << start); 
    MPU6050_WriteByte(address,subAddress,data);
} 


  
// WHO_AM_I register

/** Get Device ID.
 * This register is used to verify the identity of the device (0b110100).
 * @return Device ID (should be 0x68, 104 dec, 150 oct)
 * @see MPU6050_RA_WHO_AM_I
 */
uint8_t MPU6050_GetDeviceID()
{
    uint8_t tmp;
    tmp = MPU6050_ReadByte(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I);
    return tmp;
}

void MPU6050_Reset() 
{
  // reset device
  MPU6050_WriteBits(MPU6050_ADDRESS,MPU6050_RA_PWR_MGMT_1,MPU6050_PWR1_DEVICE_RESET_BIT,1,1);
  delayms(100);
}
/** Set clock source setting.
 * An internal 8MHz oscillator, gyroscope based clock, or external sources can
 * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
 * or an external source is chosen as the clock source, the MPU-60X0 can operate
 * in low power modes with the gyroscopes disabled.
 *
 * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
 * However, it is highly recommended that the device be configured to use one of
 * the gyroscopes (or an external clock source) as the clock reference for
 * improved stability. The clock source can be selected according to the following table:
 *
 * <pre>
 * CLK_SEL | Clock Source
 * --------+--------------------------------------
 * 0       | Internal oscillator
 * 1       | PLL with X Gyro reference
 * 2       | PLL with Y Gyro reference
 * 3       | PLL with Z Gyro reference
 * 4       | PLL with external 32.768kHz reference
 * 5       | PLL with external 19.2MHz reference
 * 6       | Reserved
 * 7       | Stops the clock and keeps the timing generator in reset
 * </pre>
 *
 * @param source New clock source setting
 * @see MPU6050_GetClockSource()
 * @see MPU6050_RA_PWR_MGMT_1
 * @see MPU6050_PWR1_CLKSEL_BIT
 * @see MPU6050_PWR1_CLKSEL_LENGTH
 */
void MPU6050_SetClockSource(uint8_t source)
{
    MPU6050_WriteBits(MPU6050_ADDRESS,MPU6050_RA_PWR_MGMT_1,MPU6050_PWR1_CLKSEL_BIT,MPU6050_PWR1_CLKSEL_LENGTH,source);
}
/** Set sleep mode status.
 * @param enabled New sleep mode enabled status
 * @see MPU6050_GetSleepModeStatus()
 * @see MPU6050_RA_PWR_MGMT_1
 * @see MPU6050_PWR1_SLEEP_BIT
 */
void MPU6050_SetSleepModeStatus(char NewState)
{
    MPU6050_WriteBits(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT,1, NewState);
}

/** Get sleep mode status.
 * Setting the SLEEP bit in the register puts the device into very low power
 * sleep mode. In this mode, only the serial interface and internal registers
 * remain active, allowing for a very low standby current. Clearing this bit
 * puts the device back into normal mode. To save power, the individual standby
 * selections for each of the gyros should be used if any gyro axis is not used
 * by the application.
 * @return Current sleep mode enabled status
 * @see MPU6050_RA_PWR_MGMT_1
 * @see MPU6050_PWR1_SLEEP_BIT
 */
char MPU6050_GetSleepModeStatus()
{
    uint8_t tmp;
    MPU6050_ReadBits(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1,&tmp);
    return tmp == 0x00 ? 0 : 1;
}

/** Set full-scale gyroscope range.
 * @param range New full-scale gyroscope range value
 * @see MPU6050_GetFullScaleGyroRange()
 * @see MPU6050_GYRO_FS_250
 * @see MPU6050_RA_GYRO_CONFIG
 * @see MPU6050_GCONFIG_FS_SEL_BIT
 * @see MPU6050_GCONFIG_FS_SEL_LENGTH
 */
void MPU6050_SetFullScaleGyroRange(uint8_t range)
{
    MPU6050_WriteBits(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH ,range);
}

// GYRO_CONFIG register

/** Get full-scale gyroscope range.
 * The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
 * as described in the table below.
 *
 * <pre>
 * 0 = +/- 250 degrees/sec
 * 1 = +/- 500 degrees/sec
 * 2 = +/- 1000 degrees/sec
 * 3 = +/- 2000 degrees/sec
 * </pre>
 *
 * @return Current full-scale gyroscope range setting
 * @see MPU6050_GYRO_FS_250
 * @see MPU6050_RA_GYRO_CONFIG
 * @see MPU6050_GCONFIG_FS_SEL_BIT
 * @see MPU6050_GCONFIG_FS_SEL_LENGTH
 */
uint8_t MPU6050_GetFullScaleGyroRange()
{
    uint8_t tmp;
    MPU6050_ReadBits(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, &tmp);
    return tmp;
}

/** Get full-scale accelerometer range.
 * The FS_SEL parameter allows setting the full-scale range of the accelerometer
 * sensors, as described in the table below.
 *
 * <pre>
 * 0 = +/- 2g
 * 1 = +/- 4g
 * 2 = +/- 8g
 * 3 = +/- 16g
 * </pre>
 *
 * @return Current full-scale accelerometer range setting
 * @see MPU6050_ACCEL_FS_2
 * @see MPU6050_RA_ACCEL_CONFIG
 * @see MPU6050_ACONFIG_AFS_SEL_BIT
 * @see MPU6050_ACONFIG_AFS_SEL_LENGTH
 */
uint8_t MPU6050_GetFullScaleAccelRange()
{
    uint8_t tmp;
    MPU6050_ReadBits(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, &tmp);
    return tmp;
}

/** Set full-scale accelerometer range.
 * @param range New full-scale accelerometer range setting
 * @see MPU6050_GetFullScaleAccelRange()
 */
void MPU6050_SetFullScaleAccelRange(uint8_t range)
{
    MPU6050_WriteBits(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}

void MPU6050_Set_LPF(uint16_t lpf)
{
	uint8_t data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6; 
	return MPU6050_WriteBits(MPU6050_ADDRESS,MPU6050_RA_CONFIG,0,3,data);//设置数字低通滤波器  
}
/** Set sample rate.
 * @param enabled New sleep mode enabled status
 * @see MPU6050_SetSampleRate()
 * @see MPU6050_RA_SMPLRT_DIV
 */
void MPU6050_SetSampleRate(uint16_t smp)
{
    uint8_t tmp;
    if(smp > 1000) smp = 1000;
    if(smp < 4) smp = 4;
    tmp = 1000/smp - 1;
    MPU6050_WriteByte(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, tmp);
    MPU6050_Set_LPF(smp/2);
}


void MPU6050_SetWork(char smp)
{
    uint8_t tmp;
    if(smp)
    {
        tmp = 0;
    }
    else
    {
        tmp = 0xff;
    }
    MPU6050_WriteBits(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2,0,6,tmp);
}

/** Power on and prepare for general usage.
 * This will activate the device and take it out of sleep mode (which must be done
 * after start-up). This function also sets both the accelerometer and the gyroscope
 * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
 * the clock source to use the X Gyro for reference, which is slightly better than
 * the default internal clock source.
 */
void MPU6050_Initialize()
{
    MPU6050_SetSleepModeStatus(0);
    // MPU6050_SetFullScaleGyroRange(MPU6050_GYRO_FS_2000);
    // MPU6050_SetFullScaleAccelRange(MPU6050_ACCEL_FS_2); 
    // MPU6050_SetSampleRate(50);

    // MPU6050_WriteByte(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0);
    // MPU6050_WriteByte(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0);
    // MPU6050_WriteByte(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0);

    // MPU6050_SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
    // MPU6050_SetWork(1);

    MPU6050_WriteByte(MPU6050_ADDRESS,MPU6050_RA_PWR_MGMT_1, 0x00);      //解除休眠状态
    MPU6050_WriteByte(MPU6050_ADDRESS,MPU6050_RA_SMPLRT_DIV , 0x07);     //陀螺仪采样率
    MPU6050_WriteByte(MPU6050_ADDRESS,MPU6050_RA_CONFIG , 0x06);
    MPU6050_WriteByte(MPU6050_ADDRESS,MPU6050_RA_ACCEL_CONFIG , 0x01);   //配置加速度传感器工作在2G模式
    MPU6050_WriteByte(MPU6050_ADDRESS,MPU6050_RA_GYRO_CONFIG, 0x18);     //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)

    // MPU6050_WriteByte(MPU6050_ADDRESS, MPU_INT_EN_REG, 0);
    // MPU6050_WriteByte(MPU6050_ADDRESS, MPU_USER_CTRL_REG, 0);
    // MPU6050_WriteByte(MPU6050_ADDRESS,MPU_FIFO_EN_REG,0X00);	//关闭FIFO
	// MPU6050_WriteByte(MPU6050_ADDRESS,MPU_PWR_MGMT2_REG,0X00);	//加速度与陀螺仪都工作
}


void mpu6050readAccelData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z accel register data stored here
  MPU6050_ReadBytes(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array
  destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
}

void mpu6050readGyroData(int16_t * destination)
{
  uint8_t rawData[6];  // x/y/z gyro register data stored here
  MPU6050_ReadBytes(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
  destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
  destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
  destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
}

int16_t mpu6050readTempData()
{
  uint8_t rawData[2];  // x/y/z gyro register data stored here
  MPU6050_ReadBytes(MPU6050_ADDRESS, MPU6050_RA_TEMP_OUT_H, 2, &rawData[0]);  // Read the two raw data registers sequentially into data array 
  return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ;  // Turn the MSB and LSB into a 16-bit value
}


void mpu6050init(void)
{
    unsigned char id = MPU6050_GetDeviceID();
    if(id == 0x68)
    {
        ESP_LOGI(TAG, "MPU IS OK");
        MPU6050_Reset();
        MPU6050_Initialize();
    }
    else
    {
        ESP_LOGI(TAG, "MPU ERR!!!");
    } 
}

void mpu6050TimeInterrupt(int arg)
{
    mpu6050Handle.read_cnt += arg; 
    if(mpu6050Handle.read_cnt >= mpu6050Handle.read_frq)
    {
        mpu6050Handle.read_cnt = 0;
        mpu6050Handle.read_flag = 1;
    }
}

void MPU6050_SetStatus(char en,int frq)
{
    mpu6050Handle.en = en;
    mpu6050Handle.read_frq = frq;
    mpu6050Handle.read_cnt = 0;
    mpu6050init();
}

void mpu6050Process(int arg)
{
    int16_t rawdata[3];
    if(mpu6050Handle.read_flag)
    {
        mpu6050Handle.read_flag = 0;
        mpu6050readAccelData(rawdata);
        mpu6050Handle.acc.x = rawdata[0]/32768.0*2.0;
        mpu6050Handle.acc.y = rawdata[1]/32768.0*2.0;
        mpu6050Handle.acc.z = rawdata[2]/32768.0*2.0;
        mpu6050readGyroData(rawdata);
        mpu6050Handle.gyro.x = rawdata[0]/32768.0*2000.0;
        mpu6050Handle.gyro.y = rawdata[1]/32768.0*2000.0;
        mpu6050Handle.gyro.z = rawdata[2]/32768.0*2000.0;
        mpu6050Handle.temperature =  ((mpu6050readTempData() /340.0))+36.53;
        //ESP_LOGI(TAG, "MPU RAW DATA IS[temp:%f] [ACC:%f %f %f][GRYO:%f %f %f]",
        // mpu6050Handle.temperature,
        // mpu6050Handle.acc.x,
        // mpu6050Handle.acc.y,
        // mpu6050Handle.acc.z,
        // mpu6050Handle.gyro.x,
        // mpu6050Handle.gyro.y,
        // mpu6050Handle.gyro.z);
    }
}